#include "commonRobotInfoDice.h"

//Declare variables and constants
#define GRAY_THRESHOLD 14

//Declare functions.
void initializeRobot();

void printValues();

void moveOffRamp(int motorPower);//Moves robot +off ramp or parking zone.

void goalAndKickstand();//Score in center goal and knock over kicstand.

void moveToKickstand();//Moves the robot from the parking zone to the kickstand.

void moveToGoals();//Moves the robot from the parking zone to the moving goals.

void getMovingGoal();//Gets the moving goal and moves it to the parking zone.

void backwardsToLine(int motorPower, int isReversed);

void rightToLine(int motorPower, int isReversed);

void rampToGoal();//Moves the robot from the ramp to the goal.

void rampToParking();//Moves the robot from the ramp to the parking zone.

task liftControl();

void moveParking();

void initializeRobot()//Initialize servos/sensors
{
	bSystemLeaveServosEnabledOnProgramStop = false;
	HTGYROstartCal(HTGYRO);
	HTEOPDsetLongRange(HTEOPD);
	StartTask(updateGyro);
	StartTask(updateSensors);
	bFloatDuringInactiveMotorPWM = false;
	//servo[Hook] = SERVO_LEFT_DOWN;
	//servo[KickArm] = KICK_IN;
	//servo[BallRedirect] = REDIRECT_UP;
	//servo[CenterGoal] = CENTER_GOAL_UP;
}

void printValues()//Prints out current sensor values (for testing)
{
	//writeDebugStreamLine("Current heading: %d", heading);
	//writeDebugStreamLine("Current distance: %d", dist);
	//writeDebugStreamLine("Current color: %d", color);
	//writeDebugStreamLine("Current direction: %d", rawIRdir);
	writeDebugStreamLine("EOPD Dist: %d", distEOPD);
	//nxtDisplayTextLine(4, "Proc:  %4d", distEOPD);
	wait10Msec(20);
}

void moveOffRamp(int motorPower)//Moves robot off ramp for autonomous routine.
{
	int currentColor = color;
	motor[Right] = motorPower * -1;
	motor[Left] = motorPower;
	motor[Rear] = motorPower * -1;
	motor[Front] = motorPower;

	time1[T1] = 0;
	while (color < GRAY_THRESHOLD && time1[T1] < 2000)
	{//While the robot hasn't passed the red/blue line.
		currentColor = color;
		wait1Msec(1);
	}
	currentColor = color;
	time1[T1] = 0;
	while (currentColor >= GRAY_THRESHOLD && time1[T1] < 1000)
	{
		currentColor = color;
		wait1Msec(1);
	}
	wait10Msec(10);
	halt();
}

void goalAndKickstand()
{
	rotateCCWGyro(30, 0);
	getIRDirection();
	if (dir == Error)//Error in reading IR sensor
	{
	}
	else if (dir == One) // if center goal is in position 1
	{

		translateDistanceBased(60, 50, 0);
		servo[CenterGoal] = CENTER_GOAL_DOWN;
		translateDistanceBased(60, 12, 180);
		rotateCWGyro(30, 45);
		servo[KickArm] = KICK_OUT;
		translateDistanceBased(60, 20, 90);
		rotateCWGyro(100, 720);
	}
	else if (dir == Two)
	{
		rotateCCWGyro(30, -90);
		translateDistanceBased(60, 40, 90);
		servo[CenterGoal] = CENTER_GOAL_DOWN;
		translateDistanceBased(60, 20, 270);
		servo[KickArm] = KICK_OUT;
		rotateCWGyro(30, -30);
		translateDistanceBased(60, 30, 90);
		rotateCWGyro(100, 720);
	}
	else
	{
		rotateCCWGyro(30, -45);
		translateDistanceBased(60, 30, 90);
		rotateCCWGyro(30, -90);
		servo[CenterGoal] = CENTER_GOAL_DOWN;
		rotateCWGyro(30, -45);
		translateDistanceBased(60, 25, 180);
		translateDistanceBased(60, 40, 90);
		rotateCWGyro(100, 720);
	}

}

void moveToKickstand()
{
	rotateCCWGyro(30, 0);
	getIRDirection();
	rotateCWGyro(30, 45);
	servo[KickArm] = KICK_OUT;
	if (dir == Error)//Error in reading IR sensor
	{
	}
	else if (dir == One) // if center goal is in position 1
	{
		translateDistanceBased(100, 10, 270);
		wait10Msec(10);
		translateDistanceBased(100, 32, 0);
		wait10Msec(20);
		translateDistanceBased(100, 8, 90);
		//translateDistanceBased(100, 30, 0);
		rotateCWGyro(100, 720);
	}
	else if (dir == Two) // if center goal is in position 2
	{
		translateDistanceBased(100, 40, 0);
		rotateCWGyro(100, 720);
	}
	else  	// if center goal is in position 3
	{
		wait10Msec(10);
		translateDistanceBased(100, 20, 90);
		wait10Msec(10);
		translateDistanceBased(100, 45, 0);
		wait10Msec(10);
		translateDistanceBased(100, 25, 270);
		wait10Msec(10);
		translateDistanceBased(100, 12, 0);
		rotateCWGyro(100, 720);
	}
	toggleKick();
}

void moveToGoals()
{
	translateDistanceBased(80, 35, 90);
	wait10Msec(5);
	motor[Left] = 100;
	motor[Right] = -100;
	while (dist > 92)//Go forward until you're close to the wall
	{
	}
	halt();
	wait10Msec(10);
	//rotateCWGyro(40, 135);
	int curDistance = dist;
	int newDist = dist;
	//rotateClockwiseAngleBased(30, 90);

	motor[Front] = 60;
	motor[Rear] = -60;
	while ((abs(newDist - curDistance)) < 4)//Go right until there is a significant change in distance (i.e. when the robot passes the moving goal)
	{
		curDistance = newDist;
		newDist = dist;
		wait10Msec(1);
	}
	if (newDist - curDistance > 0)
	{
		curDistance = dist;
		newDist = dist;
		motor[Front] = -60;
		motor[Rear] = 60;
		while ((abs(newDist - curDistance)) < 4)//Go right until there is a significant change in distance (i.e. when the robot passes the moving goal)
		{
			curDistance = newDist;
			newDist = dist;
			wait10Msec(1);
		}
	}
	halt();
}

void getMovingGoal()
{
	motor[Left] = 50;
	motor[Right] = -50;
	while (dist > 12)
	{
	}
	halt();
	wait10Msec(5);
	rotateCWGyro(50, 180);//Rotate so that the robot is in position to grab the moving goal.
	wait10Msec(5);
	motor[Right] = 30;
	motor[Left] = -30;
	motor[Rear] = 30;
	motor[Front] = -30;
	wait10Msec(50);
	halt();
	servo[Hook] = SERVO_LEFT_DOWN;//Grabs the moving goal.

	wait10Msec(20);//Moves the moving goal back to the parking zone and positions the robot.
	rotateCWGyro(50, 225);
	wait10Msec(5);
	translateDistanceBased(40, 45, 0);
	wait10Msec(5);
	translateDistanceBased(40, 22, 90);
	wait10Msec(5);
	rotateCCWGyro(30, 180);
	translateDistanceBased(60, 100, 90);
	while (color < GRAY_THRESHOLD)
	{
	}
	halt();
	rotateCCWGyro(30, 0);
	servo[Hook] = SERVO_LEFT_UP;
	rotateCWGyro(30, 45);
	rightToLine(70, -1);
	toggleHook();
	translateDistanceBased(70, 24, 270);
	backwardsToLine(60, -1);
	wait10Msec(10);
}

void backwardsToLine(int motorPower, int isReversed)//Moves the robot backwards past the red line.
{
	int currentColor = color;
	motor[Right] = motorPower * isReversed;
	motor[Left] = motorPower * -isReversed;


	while (currentColor >= GRAY_THRESHOLD)//While the robot hasn't passed the red/blue line.
	{
		currentColor = color;
		wait1Msec(1);
	}
	currentColor = color;
	while (currentColor < GRAY_THRESHOLD)
	{
		currentColor = color;
		wait1Msec(1);
	}
	halt();
}

void rightToLine(int motorPower, int isReversed)//Moves the robot right to the red line.
{
	int currentColor = color;
	motor[Front] = motorPower * isReversed;
	motor[Rear] = motorPower * -isReversed;


	while (currentColor >= GRAY_THRESHOLD)//While the robot hasn't hit the red line
	{
		currentColor = color;
		wait1Msec(1);
	}
	halt();
}

void rampToGoal()
{
	rotateCWGyro(40, 45);
	translateDistanceBased(50, 8, 270);

	int curDistance = dist;
	int newDist = dist;
	//rotateClockwiseAngleBased(30, 90);

	motor[Front] = -60;
	motor[Rear] = 60;
	while ((abs(newDist - curDistance)) < 4)//Go right until there is a significant change in distance (i.e. when the robot passes the moving goal)
	{
		curDistance = newDist;
		newDist = dist;
		wait10Msec(1);
	}
	halt();
	translateDistanceBased(40, 6, 270);
	wait10Msec(10);
	rotateCWGyro(40, 45);
	wait10Msec(10);
}

void rampToParking()//Moves from the ramp to the parking zone.
{
	rotateCWGyro(50, 45);
	wait10Msec(10);
	rotateCCWGyro(30, 45);
	translateDistanceBased(30, 8, 270);
	backwardsToLine(60, 1);
	rightToLine(60, -1);
	translateDistanceBased(70, 12, 270);
	rotateCCWGyro(30, 45);
	translateDistanceBased(70, 55, 180);
	rightToLine(70, -1);
	translateDistanceBased(70, 24, 270);
	backwardsToLine(60, -1);
	rotateCCWGyro(70, -45);
	wait10Msec(10);
}

task liftControl()//Controls raising and lowering of lift
{
	time1[T1] = 0;
	motor[Lift] = 100;
	while (time1[T1] < LIFT_TIME)//Wait 12 seconds
	{
	}
	motor[Lift] = 0;
	while (ServoValue[CenterGoal] == CENTER_GOAL_UP)//Wait for balls to be scored in center goal
	{
	}
	time1[T1] = 0;
	motor[Lift] = -100;
	while (time1[T1] < LIFT_TIME)
	{
	}
	motor[Lift] = 0;
}

void moveParking()
{
	motor[Right] = -20;
	motor[Left] = 20;
	motor[Rear] = -20;
	motor[Front] = 40;
	wait10Msec(3500);
}
